#include <stdarg.h>
#include "XBeeSerial.h"

void addArrayToArray(char* array, const char* subArray, int startIndex, int length)
{
    for (int i = 0; i < length; i++)
        array[startIndex + i] = subArray[i];
}

char* getSubArray(char* array, int startIndex, int length)
{
    char* ret = new char[length];

    for (int i = 0; i < length; i++)
    {
        ret[i] = array[startIndex + i];
    }

    return ret;
}

unsigned long bytesToInt64(char* b)
{
    unsigned long value = 0;
    for (int i = 0; i < 8; i++)
        value = (value << 8) | b[i];
    return value;
}

unsigned short bytesToInt16(char* b)
{
    unsigned short value = 0;
    for (int i = 0; i < 2; i++)
        value = (value << 8) | b[i];
    return value;
}

char* int16ToBytes(unsigned short num) {
    char* ret = new char[2];

    for (int i = 8; i >= 0; i -= 8)
    {
        ret[(8 - i) / 8] = (char)(num >> i);
    }

    return ret;
}

char* int64ToBytes(unsigned long num) {
    char* ret = new char[8];

    for (int i = 56; i >= 0; i -= 8)
    {
        ret[(56 - i) / 8] = (char)(num >> i);
    }

    return ret;
}

XBeeSerial::XBeeSerial(PinName tx, PinName rx) : Serial(tx, rx) {
    this->debugSer = NULL;
    this->enforceChecksum = true;
    attach(this, &XBeeSerial::rxHandler, Serial::RxIrq);
}

void XBeeSerial::rxHandler() {
    if(readable()) {
        if(getc() == 0x7E) {
            int len = 0;
            len = (len << 8) | getc();
            len = (len << 8) | getc();
            len++;
            
            for(int i=0; i < len; i++) {
                buffer[i] = getc();
            }
            
            XBeeResponse* xbr = new XBeeResponse(buffer, len);
            
            if(enforceChecksum && xbr->checksumFailed) {
                debug("Checksum Failure!\r\n");
                delete xbr;
            }
            else {
                if(handlerMap[xbr->apiID].isSet()) {
                    handlerMap[xbr->apiID].call(xbr);
                    delete xbr;
                }
                else {
                    respVector.insert(respVector.end(), xbr);
                }
            }
        }
    }
}

void XBeeSerial::setDebugger(Serial* debugSer) {
    this->debugSer = debugSer;
}

void XBeeSerial::debug(const char* format, ...) {
    va_list args;

    if(debugSer) {
        debugSer->printf(format, args);
    }

}

void XBeeSerial::closeOut() {
    for (vector<XBeeResponse*>::iterator it = respVector.begin(); it != respVector.end(); ++it)
        delete *it;
        
    respVector.clear();
}

bool XBeeSerial::hasData() {
    return respVector.size() > 0;
}

XBeeResponse* XBeeSerial::getResponse() {
    if(respVector.size() > 0) {
        XBeeResponse* ret = respVector.at(0);
        respVector.erase(respVector.begin());
        return ret;
    }
    
    return 0;
}

void XBeeSerial::sendLocalMessage(char apiID, char frameID, const string& msg)
{
    sendLocalMessage(apiID, frameID, msg.c_str(), msg.length());
}

void XBeeSerial::sendLocalMessage(char apiID, char frameID, const char* msg, int msgLen)
{
    XBeeRequest* xbr = new XBeeRequest(apiID, frameID, msg, msgLen);

    sendRequest(xbr);
}

void XBeeSerial::sendRemoteMessage(char apiID, char frameID, unsigned short destAddr16, const string& msg)
{
    sendRemoteMessage(apiID, frameID, destAddr16, msg.c_str(), msg.length());
}

void XBeeSerial::sendRemoteMessage(char apiID, char frameID, unsigned short destAddr16, const char* msg, int msgLen)
{
    XBeeRequest* xbr = new XBeeRequest(apiID, frameID, msg, msgLen);

    xbr->set16BitAddress(destAddr16);

    sendRequest(xbr);
}

void XBeeSerial::sendTX(char frameID, unsigned short destAddr16, const char* msg, int msgLen) {
    char *rxMsg = new char[msgLen+1];
    rxMsg[0] = 0x00;
    addArrayToArray(rxMsg, msg, 1, msgLen);
    
    sendRemoteMessage(REQ_TX_16BIT, frameID, destAddr16, rxMsg, msgLen+1);
    
    delete rxMsg;
}

void XBeeSerial::sendRequest(XBeeRequest* xbr) {
    char* req = xbr->buildRequest();
    
    writeChars(req, xbr->getLength()+4);
    
    delete req;
    delete xbr;
}

void XBeeSerial::writeChars(char* buffer, int len)
{
    debugSer->printf("Sent: ");
    
    for(int i=0; i < len; i++) {
        
        debugSer->printf("%i ", buffer[i]);

        
        putc(buffer[i]);
    }
    
    debugSer->putc('\r');
    debugSer->putc('\n');
}

void XBeeSerial::attachHandler(char apiID, void (*fptr)(XBeeResponse*)) {
    if(fptr != NULL)
        handlerMap[apiID].attach(fptr);
}

